/*
 * YawCircle.cpp
 *
 *  Created on: 2023年5月30日
 *      Author: Jiapeng Chen
 */

#include "YawCircle.hpp"

float YAW_CIRCLE::getPos_x(float t)
{
	return offset_x + A*sin(d*t);
}

float YAW_CIRCLE::getPos_y(float t)
{
	return offset_y + B - B*cos(d*t);
}

float YAW_CIRCLE::getPos_z(float t)
{
	return offset_z;
}

float YAW_CIRCLE::getYaw(float t)
{
	if(t > 360.f / 180.f * M_PI) t -=  ( t / (360.f / 180.f * M_PI) );
	return d * t;
}

void YAW_CIRCLE::setOffset(float val_x,float val_y,float val_z)
{
	offset_x = val_x;
	offset_y = val_y;
	offset_z = val_z;
}

float YAW_CIRCLE::getOffset_x()
{
	return offset_x;
}

float YAW_CIRCLE::getOffset_y()
{
	return offset_y;
}

float YAW_CIRCLE::getOffset_z()
{
	return offset_z;
}

